#include "IrSensor.h"

//#define DEBUG

IrSensor::IrSensor(const uint8_t pin)
 : my_pin(pin)
 , last_distance(INVALID_DISTANCE)
 , last_read_time(0)
{
  
}

bool IrSensor::readSensor(int & distance)
{
  distance=INVALID_DISTANCE;
  
  // if we're initializing and we've passed both the sensor init
  // time and the read delay OR we're not initializing and we've
  // passed just the sensor read delay, then read the sensor
  if ( 
      ( 0 == last_read_time && 
       ( last_read_time 
         + getSensorReadDelay() 
         + getSensorInitDelay() > millis() ) )  ||
      ( 0 != last_read_time &&
        last_read_time + getSensorReadDelay() > millis() ) )
  {
    last_read_time = millis();
    int sensorValue = analogRead(my_pin);
    last_distance = convertValueToDistance(sensorValue);
    
    distance = last_distance;
    return true;
  }

  return false;
  
}
  

inline int IrSensor::getLastSensorReading()
{
  return last_distance; 
}
inline int IrSensor::getLastSensorReadingTime()
{
  return last_read_time; 
}
inline int IrSensor::getSensorInitDelay()
{
  return 5; 
}
   
inline int IrSensor::getSensorReadDelay()
{
  return 48; 
}


struct ValueToDist {
  int value;
  int dist;
};

// We need to identify values to feed to our algorithm
// and since we know that the sensor is only ranged from
// 0 - 2.5V, we can ignore half the data points, additionally
// we're going to give a relatively small sample set (compared
// to the 1024 possible values)
ValueToDist valueDistTable[] =
 { {490, 10},
   {408, 13},
   {327, 17},
   {286, 20},
   {204, 30},
   {163, 38},
   {143, 40},
   {133, 50},
   {102, 60},
   {92,	 70},
   {82,	 80} };

int IrSensor::convertValueToDistance( int value )
{
  ValueToDist left;
  ValueToDist right;
  float slope;
  float c;
  
  int result = -1;

  // Test for the value being out of bounds
  if (value > valueDistTable[0].value || 
      value < valueDistTable[11].value )
  {
#ifdef DEBUG
    Serial.println("value out of bounds");
#endif
    result = -1;
  }
  // test for the value at the closer edge of valid values
  else if ( value == valueDistTable[0].value )
  {
    result = valueDistTable[0].dist;
  }
  // test for the value at the further edge of valid values
  else if ( value == valueDistTable[11].value)
  {
    result = valueDistTable[11].dist;
  }
  // handle all other points in between
  else
  {
    // loop through the table, except the last one
    for (int i = 0; i < 11; i++ )
    {
      // if the value is between the current and next point
      if ( value <= valueDistTable[i].value && 
           value > valueDistTable[i+1].value )
      {
#ifdef DEBUG
         Serial.print("found value range, upper =");
         Serial.print(valueDistTable[i].value);
         Serial.print(" lower =");
         Serial.println(valueDistTable[i+1].value );
#endif
         // using a simple slope algorithm to find
         // the value on the line between two known
         // points
         //   value=slope * dist + c
         left=valueDistTable[i];
         right=valueDistTable[i+1];

         // calculate the slope from the known points
         slope = ( (left.value - right.value) / (left.dist - right.dist));
         // get the slope constant using one of the known points
         c = left.value-slope*left.dist;

         // get the results from the algorithm
         result=int((value - c) / slope);

         // break out of the loop, because we've found what we're looking for
         break;
      }
    }
  }  
  
  return result;
 
  
}




